/**
 * @file ebex_math.h
 *
 * @date Sep 14, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_MATH_H_
#define EBEX_MATH_H_

#include <math.h>
#include <xmmintrin.h>
#include <float.h>

#ifndef M_PI
# define M_PI		3.1415926535897932384626433832795	/* pi */
#endif
#ifndef M_2PI
# define M_2PI		6.2831853071795864769252867665590
#endif

/// For some reason, my compiler is not getting this definition from float.h (it should!)
#ifndef DBL_EPSILON
#	define DBL_EPSILON __DBL_EPSILON__
#endif

extern double ebex_slerp_tol;

typedef struct __attribute__ ((aligned(16)))
{
	union __attribute__ ((aligned(16)))
	{
		double element[3];
		struct
		{
			double x;
			double y;
			double z;
		};
		struct
		{
			double roll;
			double pitch;
			double yaw;
		};
	};
} ebex_3vec_t;

typedef struct __attribute__ ((aligned(16)))
{
	union __attribute__ ((aligned(16)))
	{
		__m128d vec_element[2];
		double element[4];
		struct
		{
			double x;
			double y;
			double z;
			double w;
		};
	};
} ebex_4vec_t;

typedef ebex_4vec_t ebex_quat_t;

typedef struct
{
	ebex_3vec_t col[3];
} ebex_mat3x3_t;

typedef struct
{	union __attribute__ ((aligned(16)))
	{
		double		element[16];
    	ebex_4vec_t col[4];
	};
} ebex_mat4x4_t;


/**
 * Define a few stride lengths for packed arrays (these will depend on alignment)
 */
#define EBEX_3VEC_STRIDE (sizeof(ebex_3vec_t)/sizeof(double))
#define EBEX_4VEC_STRIDE (sizeof(ebex_4vec_t)/sizeof(double))
#define EBEX_QUAT_STRIDE (sizeof(ebex_quat_t)/sizeof(double))
#define EBEX_3MAT_STRIDE (sizeof(ebex_3mat_t)/sizeof(double))
#define EBEX_4MAT_STRIDE (sizeof(ebex_4mat_t)/sizeof(double))

#define EBEX_3VEC_ZERO {{0.0, 0.0, 0.0}}
#define EBEX_3MAT_IDENTITY {{{1.0, 0.0, 0.0},{0.0, 1.0, 0.0},{0.0, 0.0, 1.0}}}
#define EBEX_QUAT_IDENTITY {{0.0, 0.0, 0.0, 1.0}}


static inline double ebex_kahan_sum(size_t m_num, const double *m_vals, size_t m_step_size)
{
	double y,t;
	volatile double c = 0.0;
	double sum = 0.0;

	for (size_t i = 0; i < m_num; i++)
	{
		y = m_vals[i * m_step_size] - c;
		t = sum + y;
		c = (t - sum) - y;
		sum = t;
	}
	return sum;
}
#endif /* PFMATH_H_ */
